#include "stdafx.h"
#include "brick.hpp"

#include <stdio.h>
#include <time.h>
#include <string>
#include <stdlib.h>
using namespace std;

    Brick::Brick(){
                       
    }
    
    Brick::Brick(Serial *cp){
        comport=cp;               
    }
    
    int Brick::set_name(string name){
        char command[MAX_NR_BYTES];
        int length=name.length();
        int i;
        if(length>15){
            length=15;
        }
        /* output to bluetooth*/
        command[0]=length+3;  //command length
        command[1]=0x00;
        /*start of message*/
        command[2]=0x81;
        command[3]=0x98;
        for(i=0; i<length ;i++){
             command[i+4]=name[i];
        }
        command[i+4]='\0';
        return comport->send(&command[0],length+5);
    }
    
    string Brick::get_name(){
        int i;
        char *answer;
        char command[4];
        command[0]=0x02;  //command length
        command[1]=0x00;
        
        command[2]=0x01;
        command[3]=0x9B;
        comport->send(&command[0],4);
        answer= comport->receive(35);
        i=5;
        while( (answer[i]!='\0')){
            answer[i-5]=answer[i];
            i++;                   
        }
        answer[i-5]='\0';
        Sleep(80); //For some reason it is not possible to recieve a system command and send a direct command without delay
        return answer;
    }
    
    string Brick::firmware_version(){
        char *answer;
        char command[4];
        command[0]=0x02;  //command length
        command[1]=0x00;
        
        command[2]=0x01;
        command[3]=0x88;
        comport->send(&command[0],4);
        answer = comport->receive(9);
        answer[0]=answer[8]+48;
        answer[1]='.';
        answer[2]='0';
        answer[3]=answer[7]+48;
        answer[4]='\0';
        Sleep(80); //For some reason it is not possible to recieve a system command and send a direct command without delay
        return answer;
    }
    
    int Brick::keep_alive(){
        char command[4];
        command[0]=0x02;  //command length
        command[1]=0x00;
        
        command[2]=0x80;
        command[3]=0x0D;
        return comport->send(&command[0],4);
    }
    
    int Brick::start_program(string file){
        char *answer;
        char command[MAX_NR_BYTES];
        int length=file.length();
        int i;
        if(length>19){
            length=19;
        }
        /* output to bluetooth*/
        command[0]=length+3;  //command length
        command[1]=0x00;
        /*start of message*/
        command[2]=0x00;
        command[3]=0x00;
        for(i=0; i<length ;i++){
             command[i+4]=file[i];
        }
        command[i+4]='\0';
        comport->send(&command[0],length+5);
        answer= comport->receive(5);
        if(answer[4]){
            return -1;              
        }
        else{
            return 1;     
        }
    }
     int Brick::stop_program(){
        char command[4];
        command[0]=0x02;  //command length
        command[1]=0x00;
        
        command[2]=0x80;
        command[3]=0x01;
        return comport->send(&command[0],4);
    }
    
    int Brick::battery_level(){
        char *answer;
        char command[4];
        int mvolt;
        command[0]=0x02;  //command length
        command[1]=0x00;
        
        command[2]=0x00;
        command[3]=0x0B;
        comport->send(&command[0],4);
        answer= comport->receive(8);
        mvolt = ((0xFF & answer[5]) | (answer[6] << 8));
        Sleep(80); //For some reason it is not possible to recieve a system command and send a direct command without delay
        return mvolt;
    }
    
    
